#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S2,     HTSMUX,                sensorLowSpeed)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S1_C2_1,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    ,                     tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    ,                     tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "LEGOUS-driver.h"
#include "HTGYRO-driver.h"
#include "HTAC-driver.h"
void autonomousAtTheBridge()
{
  int tiltduration = 0;
  ClearTimer(T1);
  servoChangeRate[servo1] = 1;
  servoChangeRate[servo3] = 1;
  servoChangeRate[servo4] = 10;
  servoChangeRate[servo5] = 10;
  servoChangeRate[servo6] = 1;
  HTSMUXinit();
  HTSMUXscanPorts(HTSMUX);
  motor[motorD] = 0;
  motor[motorE] = 0;
  wait1Msec(500);// calibrate gyro for bridge balancing code
  HTGYROstartCal(msensor_S2_4);
  int _x_axis1 = 0;
  int _y_axis1 = 0;
  int _z_axis1 = 0;
  HTACreadX(msensor_S2_1, _x_axis1);
  HTACreadY(msensor_S2_1, _y_axis1);
  HTACreadZ(msensor_S2_1, _z_axis1);
  HTGYROstartCal(msensor_S2_4);
  while (time1[T1] < 14000/* wait until a robot get on the bridge*/ && tiltduration < 3/* waits until the robot is tilted for 30 msec*/)
  {
    HTACreadX(msensor_S2_1, _x_axis1);
    HTACreadY(msensor_S2_1, _y_axis1);
    HTACreadZ(msensor_S2_1, _z_axis1);
    if (_z_axis1 > 10)// if the robot detects a forward force, increment tiltduration. the bridge being bumped can trigger this, so it has to detect three accelerations
    {
      ++tiltduration;
    }
    else// if the bridge is not tilted by an enemy robot, reset tiltduration to 0
    {
      tiltduration = 0;
    }
    wait1Msec(10);
  }
  servo[servo1] = 250;
  if (_z_axis1 < -5)
  {
	  HTGYROstartCal(msensor_S2_4);
	  wait1Msec(1000);
	}
  if (_z_axis1 < -5)// if the bridge isn't tilted by another robot
  {
    servo[servo5] = 10;// move drop away lexan in
    servo[servo4] = 245;
    short nRawValues[4];//create color sensor values
    getColorSensorData(colorPort, colorRaw,    &nRawValues);
    wait1Msec(500);
    servo[servo6] = 30;// drop scoop
    servo[servo3] = 225;
    wait1Msec(1000);//wait for scoop to finish moving
    //drive forward to line
    nMotorEncoder[motorD] = 0;
    motor[motorD] = 20;
    motor[motorE] = 20;
    getColorSensorData(colorPort, colorRaw,    &nRawValues);
    while (abs(nRawValues[0] - nRawValues[2]) && nMotorEncoder[motorD] < 1575)
    {
      getColorSensorData(colorPort, colorRaw,    &nRawValues);
    }
    motor[motorD] = 0;
    motor[motorE] = 0;
	  wait1Msec(500);
	  while (true)
	  {
	    // start balancing code
	    nxtDisplayTextLine(4, "Gyro:   %4d", HTGYROreadRot(msensor_S2_4));
	    nxtDisplayTextLine(5, "Accelerometer: %4d", _z_axis1);\
	    wait1Msec(75);
	    motor[motorD] = 0;
	    motor[motorE] = 0;
	    wait1Msec(300);
	    HTACreadX(msensor_S2_1, _x_axis1);
      HTACreadY(msensor_S2_1, _y_axis1);
      HTACreadZ(msensor_S2_1, _z_axis1);
	    if (HTGYROreadRot(msensor_S2_4) < 3 && HTGYROreadRot(msensor_S2_4) > -3)// if the bridge is not moving, try to balance
	    {
	      if (_z_axis1 > 15)// if bridge is tilted forward, move backward
	      {
	        motor[motorD] = -20;
	        motor[motorE] = -20;
	      }
	      else if (_z_axis1 < -15)// if bridge is tilted backward, move forward
	      {
	        motor[motorD] = 20;
	        motor[motorE] = 20;
	      }
	      else// if bridge is stable, stop moving
	      {
	        motor[motorD] = 0;
	        motor[motorE] = 0;
	      }
	    }
	    else
	    {
	      motor[motorD] = 0;
	      motor[motorE] = 0;
	    }
	  }
	}
	else
	{
	  // defensive autonomous
	  servoChangeRate[servo4] = 10;
    servoChangeRate[servo5] = 10;
    nMotorEncoder[motorE] = 0;
	  nMotorEncoder[motorD] = 0;
	  nMotorEncoderTarget[motorE] = USreadDist(msensor_S2_3)*42 - 200;// read the SONAR sensor, and calculate degrees to rotate for motors so the robot stops in front of enemy
	  nMotorEncoderTarget[motorD] = USreadDist(msensor_S2_3)*42 - 200;
	  servo[servo5] = 10;
    servo[servo4] = 245;
	  wait1Msec(1000);
	  motor[motorD] = 50;// move forward
	  motor[motorE] = 50;
	  ClearTimer(T2);
	  while (nMotorRunState[motorE] != runStateIdle && nMotorRunState[motorD] != runStateIdle && time1[T2] < 5000);

	  motor[motorD] = 0;
	  motor[motorE] = 0;
	  wait1Msec(20000);// sit on bridge so the enemy doesnt get across
	}
}
